"""
# pytest tests/ShapePhysics_test.py
物理碰撞算法测试

- 测试算法: GJK
"""

from ShapePhysics import ShapeBase, BoxCheck, GJKCheck, Simplex, np, show_ls, EPA_Algorithom, Edge, EPAPolygon
from plot_polygon import plot_polygon_ls, plot_polygon


def _test_core(aa, bb, simp=None):
    # --- 核心测试函数
    if not isinstance(aa, ShapeBase) or not isinstance(bb, ShapeBase):
        aa: ShapeBase = ShapeBase(aa)
        bb: ShapeBase = ShapeBase(bb)

    # box_collision = BoxCheck(aa, bb)

    if simp is None:
        simp = Simplex()  # 记录包含原点的单纯形, 用EPA来反推碰撞向量
    is_collision = GJKCheck(aa, bb, simp)
    print('--- is_collision: ', is_collision)
    return is_collision


def test_overlap():
    """
    重叠情形测试
    """
    aa = 2 * np.array([
        [1, 1],
        [-1, 1],
        [-1, -1],
        [1, -1],
    ])

    bb = np.array([
        [2, 0],
        [2, -3],
        [3, 3],
        [3, -3],
    ])

    simp = Simplex()        # 记录包含原点的单纯形
    is_collision = _test_core(aa, bb, simp)
    assert is_collision is True, 'Return Value Error!'

    EPA_Algorithom(aa, bb, simp)


def test_penetrate():
    """
    穿透情形测试
    """
    aa = 2 * np.array([
        [1, 1],
        [-1, 1],
        [-1, -1],
        [1, -1],
    ])

    bb = np.array([
        [1, 0],
        # [2, -3],
        [3, 3],
        [3, -3],
    ])

    simp = Simplex()        # 记录包含原点的单纯形
    is_collision = _test_core(aa, bb, simp)
    assert is_collision is True, 'Return Value Error!'

    return simp


def _test_EPA_core(aa, bb):
    simp = Simplex()  # 记录包含原点的单纯形
    is_collision = _test_core(aa, bb, simp)
    assert is_collision is True, 'Return Value Error!'

    plot_polygon(np.array(simp.ls))

    # --- EPA算法计算穿透向量
    ret = EPA_Algorithom(aa, bb, simp)
    print('--- ret, norm:', ret, ',', round(np.linalg.norm(ret), 2))
    return ret


def test_penetrate_epa():
    """
    穿透情形测试
    """
    aa = 2 * np.array([
        [1, 1],
        [-1, 1],
        [-1, -1],
        [1, -1],
    ])

    bb = np.array([
        # [1, 2],
        # [1, -2],
        [1.6, 0],     # 三角形
        [3, 3],
        [3, -3],
    ])

    if 0:
        # 穿刺向量应为[1, 1], 但只能检测出一个方向... 有待改进.
        aa = 1 * np.array([
            [1, 1],
            [-1, 1],
            [-1, -1],
            [1, -1],
        ])

        bb = 2 * np.array([
            [0, 0.],
            [1, 0.],
            # [1.5, 0],     # 三角形
            [1, 1],
            [0, 1],
        ])

    if 0:
        # 知乎案例
        aa = ShapeBase([
            [2, 3],
            [3, 4],
            [5, 3],
            [4, 1],
        ], position=[4, 2])

        bb = ShapeBase([
            [3, 3],
            [1, 1],
            [0, 2],
            [1, 4],
        ], position=[1, 3])

    plot_polygon_ls([aa, bb], ['yellow', 'blue'], alpha=0.3)

    ret = _test_EPA_core(aa, bb)

    return ret


def test_non_overlap():
    """
    不重叠情形测试, 近距离
    """
    aa = 2 * np.array([
        [1, 1],
        [-1, 1],
        [-1, -1],
        [1, -1],
    ])

    bb = np.array([
        [2.0001, 0],
        [3, 3],
        [3, -3],
    ])

    is_collision = _test_core(aa, bb)
    assert is_collision is False, 'Return Value Error!'


def test_non_overlap_in_big_distance():
    """
    不重叠情形测试, 远距离
    """
    aa = 2 * np.array([
        [1, 1],
        [-1, 1],
        [-1, -1],
        [1, -1],
    ])

    bb = 30 * np.array([
        [2, 0],
        [3, 3],
        [3, -3],
    ])

    is_collision = _test_core(aa, bb)
    assert is_collision is False, 'Return Value Error!'


if __name__ == '__main__':
    # test_overlap()
    # test_penetrate()
    test_penetrate_epa()
    # test_non_overlap()
    # test_non_overlap_in_big_distance()


    if 0:
        def getFarthestPointInDirection(aa, d):
            # 多边形aa在d方向上最远的点
            index_max = aa.dot(d).argmax()
            ret = aa[index_max]
            # aa = np.delete(aa, index_max, axis=0)     # 删除用过的点, 但函数内部变量暂时无法实现
            index_ls = list(range(0, aa.shape[0]))
            index_ls.remove(index_max)
            aa = aa[index_ls, :]

            return ret


        if 1:
            import numpy as np

            aa = np.array([
                [1, 1, 1],
                [2, 2, 2],
                [3, 3, 3],
                [4, 4, 4],
            ])
            # index_max = 1
            # index_ls = list(range(0, aa.shape[0]))
            # index_ls.remove(index_max)
            # aa = aa[index_ls, :]
            getFarthestPointInDirection(aa, [0, 0, 1])
            aa

            1